#include <Key.h>
#include <Keypad.h>
#include <Servo.h> 
#include "HX711.h"
#include <Wire.h> //I2C
#include <Adafruit_PWMServoDriver.h>

#include "stdio.h"
#include "string.h"
#include "stdlib.h"

// 引入驱动OLED0.96所需的库
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // 设置OLED宽度,单位:像素
#define SCREEN_HEIGHT 64 // 设置OLED高度,单位:像素

// 自定义重置引脚,虽然教程未使用,但却是Adafruit_SSD1306库文件所必需的
#define OLED_RESET 4

const byte ROWS = 4; //four rows
const byte COLS = 4; //four columns
//使用servo库
Servo a,b,c,d,e,f;    //创建6个舵机对象
//存储电机极限值(const指定该数值为常量,常量数值在程序运行中不能改变)
const int aMin = 0;//张开
const int aMax = 80;//加紧
const int bMin = 0;//左
const int bMax = 180;//右
const int cMin = 0;//前
const int cMax = 180;//后
const int dMin = 0;//前
const int dMax = 180;//后
const int eMin = 0;//前
const int eMax = 180;//后
const int fMin = 0;//逆时针
const int fMax = 180;//顺时针

char hexaKeys[ROWS][COLS] = {
  {'0','1','2','3'},
  {'4','5','6','7'},
  {'8','9','A','B'},
  {'C','D','E','F'}
};
  
byte rowPins[ROWS] = {11,10,9,8};
byte colPins[COLS] = {4,5,6,7};
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//initialize an instance of class NewKeypad
Keypad customKeypad = Keypad( makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS); 

//默认地址0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVOMIN 102
#define SERVOMAX 512

int DSDa=2;
int DSDb=2;
int DSDc=2;
int DSDd=2;
int DSDe=2;
int DSDf=2;//Default Servo Delay (默认电机运动延迟时间)
              //此变量用于控制电机运行速度.增大此变量数值将
              //降低电机运行速度从而控制机械臂动作速度。
float convert(float afa);
void words_display(void);

float Weight = 0;
float unit_price = 0.0;
float price = 0.0;
int rank = 0;
int state = 1;//state = 1:正常称重    state = 0:设置单价 state = 2 机械臂运动
char newKey = 0;

void setup()
{
	Init_Hx711();				//初始化HX711模块连接的IO设置
  // 初始化Wire库
  Wire.begin();
  
  // 初始化OLED并设置其IIC地址为 0x3C
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
	Serial.begin(9600);//串口3波特率9600
  Serial.flush();
  pwm.begin();
  pwm.setPWMFreq(50);
  
	delay(1000);
	Get_Maopi();		//获取毛皮
}

void loop()
{
  /*****确定工作状态*********/
  char customKey = customKeypad.getKey();
  
  /*****根据工作状态********/
  if(state == 1){
	  Weight = Get_Weight();	//计算放在传感器上的重物重量
    if(Weight >= 500){
      rank = 1;
      }
     else if(Weight >= 200){
      rank = 2;
      }
     else if(Weight > 100){
        rank = 3;
        }
     else{
        rank = 0;
      }
    price = unit_price*Weight/1000.0;
	  Serial.print(float(Weight/1000.0),3);	//串口显示重量
	  Serial.print("\r\n");
    words_display();
    display.display();
    delay(300);        //延时0.3s
  }
}

void words_display(void)
{
  char oled_buf[20]={0};
    // 清除屏幕
  display.clearDisplay();
  // 设置字体颜色,白色可见
  display.setTextColor(WHITE);
   //设置字体大小
  display.setTextSize(1.5);
  //设置光标位置
  display.setCursor(0,0);
  display.print("Fruit Steelyard");
  
  display.setCursor(0,10);
  sprintf(oled_buf,"Unit: %d.%d",(int)unit_price,((int)unit_price*10)%10);
  display.print(oled_buf);
  memset(oled_buf,0,strlen(oled_buf));
  
  display.setCursor(0,20);
  sprintf(oled_buf,"weight: %d.%d",(int)Weight,((int)Weight*10)%10);
  display.print(oled_buf);
  memset(oled_buf,0,strlen(oled_buf));
  
  display.setCursor(0,30);
  sprintf(oled_buf,"rank: %d",rank);
  display.print(oled_buf);
  memset(oled_buf,0,strlen(oled_buf));
  
  display.setCursor(0,40);
  sprintf(oled_buf,"price: %d.%d",(int)price,((int)price*10)%10);
  display.print(oled_buf);
  memset(oled_buf,0,strlen(oled_buf));
 
  }

//舵机控制板
void setServoPulse(int n,double pulse){
  double pulselength;
  pulselength = 1000000;//1000000us==1s
  pulselength /= 50;//50Hz
  pulselength /= 4096;//12位分辨率
  pulse *= 1000;
  pulse /= pulselength;
  pwm.setPWM(n,0,pulse);
  }
  
//控制舵机旋转角度
/*
*n代表第几路舵机
*/
void servo_angle(int n,int Angle){
  double pulse = Angle;
  pulse = (double)(pulse/90.0) + 0.5;
  setServoPulse(n,pulse);
  }
//动作组
void actiongroup1()
{     zhuaqu();
      taidabi();
      taixiaobi();
      taixiaobi2();
      fangzhi1();
      songzhua();
      taidabi();
      armIniPos();
  }

void actiongroup2()
{     zhuaqu();
      taidabi();
      taixiaobi();
      taixiaobi2();
      fangzhi2();
      songzhua();
      taidabi();
      armIniPos();
  }  

void actiongroup3()
{     zhuaqu();
      taidabi();
      taixiaobi();
      taixiaobi2();
      fangzhi3();
      songzhua();
      taidabi();
      armIniPos();
  }  


/********************动作设置***********************/
void arm_init(void)
{
  /****机械臂初始化*******/
  a.attach(5,500,2500);     // a 伺服舵机连接引脚5 舵机代号'a' //脉冲范围为0.5~2.5ms
  delay(100);          // 稳定性等待
  b.attach(6,500,2500);     // b 伺服舵机连接引脚6 舵机代号'b'
  delay(100);          // 稳定性等待
  c.attach(7,500,2500);      // c 伺服舵机连接引脚7  舵机代号'c'
  delay(100);          // 稳定性等待
  d.attach(8,500,2500);      // d 伺服舵机连接引脚8  舵机代号'd'
  delay(100);          // 稳定性等待
  e.attach(9,500,2500);      // e 伺服舵机连接引脚9  舵机代号'e'
  delay(100);          // 稳定性等待
  f.attach(10,500,2500);      // f 伺服舵机连接引脚10  舵机代号'f'
  delay(100);          // 稳定性等待
  servocontrol(10,90,50,35,150,90);
   delay(300);
  }
  
void zhuaqu()
  {float a_topos = 65;
  float b_topos = b.read();
  float c_topos = c.read();
  float d_topos = d.read();
  float e_topos = e.read();
  float f_topos = f.read();
  servocontrol( a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
    }
    
 void fangzhi1()//red
 {
  float a_topos = a.read();
  servocontrol( a_topos,90,90,60,40,40);
  }
  
  void fangzhi2()//green
 {
  float a_topos = a.read();
  servocontrol( a_topos,90,40,30,75,15);
  }
  
  void fangzhi3()//blue
 {
  float a_topos = a.read();
  servocontrol( a_topos,90,40,30,75,165);
  }
  
  void songzhua()
  {
  float a_topos = 10;
  float b_topos = b.read();
  float c_topos = c.read();
  float d_topos = d.read();
  float e_topos = e.read();
  float f_topos = f.read();
  servocontrol( a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  }
  
void taidabi()
{ float a_topos = a.read();
  float b_topos = b.read();
  float c_topos = c.read();
  float d_topos = d.read();
  float e_topos = 90;
  float f_topos = f.read();
  servocontrol( a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  }
  
void taixiaobi()
{ float a_topos = a.read();
  float b_topos = b.read();
  float c_topos = c.read();
  float d_topos = 40;
  float e_topos = e.read();
  float f_topos = f.read();
  servocontrol( a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  }
  
  void taixiaobi2()
{ float a_topos = a.read();
  float b_topos = b.read();
  float c_topos = 90;
  float d_topos = d.read();
  float e_topos = e.read();
  float f_topos = f.read();
  servocontrol( a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  }
  
  /************************change************************************/
 void change_a(void)
 {
  float a_topos = aMin;
  float b_topos = b.read();
  float c_topos = c.read();
  float d_topos = d.read();
  float e_topos = e.read();
  float f_topos = f.read();
  while(a_topos < aMax){
   servocontrol(a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
   a_topos++;
   delay(1);
  }
 }

void change_b(void)
 {
  float a_topos = a.read();
  float b_topos = bMin;
  float c_topos = c.read();
  float d_topos = d.read();
  float e_topos = e.read();
  float f_topos = f.read();
  servocontrol(a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  delay(500);
  b_topos = bMin;
  servocontrol(a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  delay(500);
  }

  void change_c(void)
 {
  float a_topos = a.read();
  float b_topos = b.read();
  float c_topos = cMin;
  float d_topos = d.read();
  float e_topos = e.read();
  float f_topos = f.read();
  servocontrol(a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  delay(500);
  c_topos = cMax;
  servocontrol(a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  delay(500);
  }

  void change_d(void)
{
  float a_topos = a.read();
  float b_topos = b.read();
  float c_topos = c.read();
  float d_topos = dMin;
  float e_topos = e.read();
  float f_topos = f.read();
  servocontrol(a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  delay(500);
  d_topos = dMax;
  servocontrol(a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  delay(500);
 }

  void change_e(void)
{
  float a_topos = a.read();
  float b_topos = b.read();
  float c_topos = c.read();
  float d_topos = d.read();
  float e_topos = eMin;
  float f_topos = f.read();
  servocontrol(a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  delay(500);
  e_topos = eMax;
  servocontrol(a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  delay(500);
 }

void change_f(void)
{
  float a_topos = a.read();
  float b_topos = b.read();
  float c_topos = c.read();
  float d_topos = d.read();
  float e_topos = e.read();
  float f_topos = fMin;
  servocontrol(a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  delay(500);
  f_topos = fMax;
  servocontrol(a_topos,b_topos,c_topos,d_topos,e_topos,f_topos);
  delay(500);
 }
 
  /************************************************************/
//舵机控制
void servocontrol(float a_angle, float b_angle, float c_angle,
                  float d_angle, float e_angle, float f_angle)
{ float a_frompos = a.read();
  float b_frompos = b.read();
  float c_frompos = c.read();
  float d_frompos = d.read();
  float e_frompos = e.read();
  float f_frompos = f.read(); 
  if (a_frompos <= a_angle)
  {
    for (float ia=a_frompos;ia<=a_angle;ia+=0.1)
    {
      a.write(ia);
      delay (DSDa);
      }
  }
  if (a_frompos > a_angle)
  {
    for (float ia=a_frompos;ia>=a_angle;ia-=0.18)
    {
      a.write(ia);
      delay (DSDa);
      }
  }
   if (f_frompos <= f_angle)
  {
    for (float ig=f_frompos;ig<=f_angle;ig+=0.18)
    {
      f.write(ig);
      delay (DSDf);
      }
  }
  if (f_frompos > f_angle)
  {
    for (float ig=f_frompos;ig>=f_angle;ig-=0.18)
    {
      f.write(ig);
      delay (DSDf);
      }
  }
  if (b_frompos <= b_angle)
  {
    for (float ib=b_frompos;ib<=b_angle;ib+=0.18)
    {
      b.write(ib);
      delay (DSDb);
      }
  }
  if (b_frompos > b_angle)
  {
    for (float ib=b_frompos;ib>=b_angle;ib-=0.18)
    {
      b.write(ib);
      delay (DSDb);
      }
  }
  if (d_frompos <= d_angle)
  {
    for (float id=d_frompos;id<=d_angle;id+=0.18)
    {
      d.write(id);
      delay (DSDd);
      }
  }
  if (d_frompos > d_angle)
  {
    for (float id=d_frompos;id>=d_angle;id-=0.18)
    {
      d.write(id);
      delay (DSDd);
      }
  } 
 
    if (e_frompos <= e_angle)
  {
    for (float ie=e_frompos;ie<=e_angle;ie+=0.18)
    {
      e.write(ie);
      delay (DSDe);
      }
  }
  if (e_frompos > e_angle)
  {
    for (float ie=e_frompos;ie>=e_angle;ie-=0.18)
    {
      e.write(ie);
      delay (DSDe);
      }
  } 
    if (c_frompos <= c_angle)
  {
    for (float ic=c_frompos;ic<=c_angle;ic+=0.18)
    {
      c.write(ic);
      delay (DSDc);
      }
  }
  if (c_frompos > c_angle)
  {
    for (float ic=c_frompos;ic>=c_angle;ic-=0.18)
    {
      c.write(ic);
      delay (DSDc);
      }
  } 
 }

  void armIniPos(){
//  Serial.println("复位");
   servocontrol(10,90,50,35,150,90);
   delay(1000);
}
